/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file XMLtoData.cxx
	\brief class implemenation XML parser

									  \author Jack Elston
						 $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "piccoloParser.h"
	/*<-------Defines--------->*/
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		extern bool verbose;
		extern String vehicleID;

		static int lastTrackWaypoint = -1;

		uint16_t network_id;
		bool sent_control;
		WindData_t wind;
		Telemetry_t telem;
	/*<-End Global Variables-->*/

PiccoloParser::PiccoloParser(String name, bool gs_backup) : ThreadedPipe(name)
{
	sent_control = 0;
	init         = 0;
	network_id   = 0;

	this->gs_backup = gs_backup;

	ClearAllFPMaskBits(active_wp_mask);
	for(int i=0; i < NUM_FP_POINTS; i++) {
		piccolo_waypoints[i].Next = 0xFF;
	}

	wind.south = 0.0;
	wind.west = 0.0;
	wind.gps.utc.hours = 0;
	wind.gps.utc.minutes = 0;
	wind.gps.utc.seconds = 0;
	wind.gps.pos.lat = 0;
	wind.gps.pos.lon = 0;
	wind.gps.pos.alt = 0;
	wind.gps.sog = 0;
	wind.gps.track = 0;
	wind.gps.accuracy.sats = 0;
	wind.gps.accuracy.quality = 0;
	wind.gps.accuracy.pdop = 0;

  lost_comm = FALSE;
}

/************************************************************\
|                 XMLtoData_Parser::update                   |
\************************************************************/
/*!
	\brief function called periodically by the thread.
	\pre instance of XMLtoData_Parser has been created and initialized
	\post any data in the inetRxFifo will be parsed and output in
	 command format to the SAVTxFifo
 */
void PiccoloParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	if(!init && network_id != 0) {
		AutopilotPkt_t autopilot_pkt;
		StreamPkt_t stream_pkt;

		//-- setup bandwidth mode of period packets --//
		BandwidthMode_t bwMode;
		bwMode.LoRes       = 0;
		//bwMode.RegularMode = 0x10;
		bwMode.RegularMode = 0x04;
		bwMode.Link        = 0xFF;

		FormBandwidthModePacket(&autopilot_pkt, bwMode);
		MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
					(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
		memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
		fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
		writeFifo(OUTBOARD_TX);

		//-- request active list of waypoints --//
		if(verbose)
			cout << "PiccoloParser::update - requesting waypoint list" << endl;

		SetAllFPMaskBits(active_wp_mask);
		FormWaypointListPacket(&autopilot_pkt, active_wp_mask, REQUEST_WAYPOINTS);
		MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
				(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + AUTOPILOT_PKT_OVERHEAD);
		memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
		fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
		writeFifo(OUTBOARD_TX);
		ClearAllFPMaskBits(active_wp_mask);

		init = 1;
	}

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("PiccoloParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);

			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length == fifo_msg.size && length < 1000) {
					uint8_t id = CMD_CMD(fifo_msg.data);
					if(id == CMD_COMM || id == CMD_ACK || id == CMD_FAIL) {
						Command cmd((unsigned char *)fifo_msg.data);
						if(cmd.isValid())
							parse(cmd);
						else
							parse(n);
					}
					else
						parse(n);
				}
				else
					parse(n);
			}
		}

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);

			if( n > 0 ) {

				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
							 << "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;
				
					if( length < 1000 && length <= fifo_msg.size ) {
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((unsigned char *)fifo_msg.data);
				if(!cmd.isValid())
					cout << getName() << "::update() - invalid command"<< endl << cmd ;
				else
					parse(cmd);
			}
		}
	}
}

int PiccoloParser::parse(int num)
{
	static StreamPkt_t in_stream_pkt;
	StreamPkt_t        out_stream_pkt;
	UInt32             numused;
	AutopilotPkt_t     autopilot_pkt;
	UserData_t         pkt_data;
	SystemDiagnostic_t system_pkt;
	NavDiagnostic_t    nav_pkt;
	AutopilotStatus_t  status_pkt;
	BOOL               long_pkt; 

	int                ret_val = 0;
	uint8_t            data_from_fifo[FIFO_BUF_SIZE];
	int                received_waypoint = -1;

	Client * me = shmPtr->clients.getClient(0);

	if( num != fifo_msg.size ) {
		printf("PiccoloParser::parse - num != fifo_msg.size \n");
		return -1;
	}

	// we need to create a local copy of the fifo_msg buffer, since this might
	// be used later to send a message before we completely parsed the buffer
	if( fifo_msg.size > FIFO_BUF_SIZE ){
		printf("PiccoloParser::parse - fifo_msg.size > FIFO_BUF_SIZE\n");
		return -1;
	}
	memcpy( data_from_fifo, fifo_msg.data, fifo_msg.size );

#ifdef DEBUG_PRINT
	cout << "PiccoloParser::parse - num=" << num << " [bytes]" << endl;
#endif
	for(int n = 0; n < num; n++)
	{
		int lookFor = LookForStreamPacketInByte(&in_stream_pkt,data_from_fifo[n]);
		if( lookFor == 0 ) {
#ifdef DEBUG_PRINT
			cout << "\tFound stream packet at n=" << n << endl;
#endif
			uint16_t tmp_id = (uint16_t)in_stream_pkt.SourceLo + ((uint16_t)in_stream_pkt.SourceHi << 8);
			if(tmp_id == 0) return -1; // GROUND_STATION

			if(!gs_backup)
				network_id = tmp_id;
			else {
				if(tmp_id != network_id) {
					if(verbose)
						cout << "PiccoloParser::parse - Not Address of Backup Client" << endl;
					return -1; // NOT BACKUP_CLIENT
				}
			}

			switch(in_stream_pkt.Stream){
				case AUTOPILOT_STREAM: {
					int valid;
					int ind = 0;
					do {
						// pull out the autopilot packet(s) from the stream packet
						valid = LookForAutopilotPacketInBuf((uint8_t *)&(in_stream_pkt.Data[ind]), &autopilot_pkt, 
																			 in_stream_pkt.Size-ind, &numused);
						ind += numused;

						// if valid, parse packet type
						if(valid) 
						{ 
							// detect packet type
							switch(autopilot_pkt.PktType)
							{

								case TELEMETRY_HI_RES:
									if(verbose)
										cout << getName() << "::parse() - TELEMETRY_HI_RES" << endl;

									if( !DecodeTelemetryHiResPacket(&autopilot_pkt, &pkt_data) ) {
										cout << getName() << "::parse() - TELEMETRY_HI_RES error parsing" << endl;
										break;
									}
									telem.address = network_id;

									telem.gps.date.year   = pkt_data.GPS.year;
									telem.gps.date.month  = pkt_data.GPS.month;
									telem.gps.date.day    = pkt_data.GPS.day;

									telem.gps.utc.hours   = pkt_data.GPS.hours;
									telem.gps.utc.minutes = pkt_data.GPS.minutes;
									telem.gps.utc.seconds = pkt_data.GPS.seconds;

									// convert to [deg]
									telem.gps.pos.lat     = pkt_data.GPS.Latitude*180.0/M_PI;
									telem.gps.pos.lon     = pkt_data.GPS.Longitude*180.0/M_PI;
									telem.gps.track       = pkt_data.GPS.Direction*180.0/M_PI;
									telem.gps.pos.alt     = pkt_data.GPS.Altitude;
									telem.gps.sog         = pkt_data.GPS.Speed;
									telem.gps.accuracy.pdop    = pkt_data.GPS.PDOP;
									telem.gps.accuracy.sats    = pkt_data.GPS.TrackedSats;
									telem.gps.accuracy.quality = pkt_data.GPS.Status;

									telem.airdata.alt     = pkt_data.Alt;
									telem.airdata.tas     = pkt_data.TAS;

									telem.airdata.pstatic = pkt_data.Pstatic;
									telem.airdata.pdynamic = pkt_data.Pstatic;
									telem.airdata.ias     = pkt_data.IAS;

									// convert to [deg]
								#if 0
									me->telemetry.angles.roll     = (180.0/M_PI)*(pkt_data.Euler[ROLL_AXIS]);
									me->telemetry.angles.pitch    = (180.0/M_PI)*(pkt_data.Euler[PITCH_AXIS]);
									me->telemetry.angles.yaw      = (180.0/M_PI)*(pkt_data.Euler[YAW_AXIS]);
								#else
									// convert to [millirad]
									telem.angles.roll     = (int16_t)(1000.0*pkt_data.Euler[ROLL_AXIS]);
									telem.angles.pitch    = (int16_t)(1000.0*pkt_data.Euler[PITCH_AXIS]);
									telem.angles.yaw      = (int16_t)(1000.0*pkt_data.Euler[YAW_AXIS]);
								#endif

									wind.south           = pkt_data.WindSouth;
									wind.west            = pkt_data.WindWest;
									wind.gps.utc.hours   = telem.gps.utc.hours;
									wind.gps.utc.minutes = telem.gps.utc.minutes;
									wind.gps.utc.seconds = telem.gps.utc.seconds;
									wind.gps.pos.lat     = telem.gps.pos.lat;
									wind.gps.pos.lon     = telem.gps.pos.lon;
									wind.gps.pos.alt     = telem.gps.pos.alt;
									wind.gps.sog         = telem.gps.sog;
									wind.gps.track       = telem.gps.track;


									telem.limits.altitudeLow  = pkt_data.Limits.AltitudeLow;
									telem.limits.altitudeHigh = pkt_data.Limits.AltitudeHigh;
									telem.limits.IASLow       = pkt_data.Limits.IASLow;
									telem.limits.IASHigh      = pkt_data.Limits.IASHigh;
									telem.limits.rollHigh     = pkt_data.Limits.RollHigh;
									telem.limits.pitchHigh    = pkt_data.Limits.PitchHigh;
									telem.limits.RPMLow       = pkt_data.Limits.RPMLow;
									telem.limits.RPMHigh      = pkt_data.Limits.RPMHigh;
									telem.limits.windsHigh    = pkt_data.Limits.WindsHigh;

									for(int itr = 0; itr < 4; itr ++)
										telem.servos.channels[itr] = 0;  //set to 0 for now

									telem.servos.channels[2] = (uint16_t)(pkt_data.Actuators[2]*65535); //FIXME -- only works for throttle


									sendTelem();
									break;

								case TELEMETRY_LO_RES:
									if(verbose)
										cout << getName() << "::parse() - TELEMETRY_LO_RES" << endl;
									break;

								case SYSTEM_STATUS_HI_RES:
									if(verbose)
										cout << getName() << "::parse() - SYSTEM_STATUS_HI_RES" << endl;

									if( !DecodeSystemStatusHiResPacket(&autopilot_pkt, &system_pkt, &nav_pkt, &status_pkt, &long_pkt) ) {
										cout << getName() << "::parse() - SYSTEM_STATUS_HI_RES error parsing" << endl;
										break;
									}

									// convert to [mV] and [mA]
									telem.system.twelveV    = (uint16_t)(1000*system_pkt.InputVoltage);
									telem.system.twelveA    = (uint16_t)(1000*system_pkt.InputCurrent);
									telem.system.fiveV      = (uint16_t)(1000*system_pkt.ServoVoltage);
									telem.system.fiveA      = (uint16_t)(1000*system_pkt.ServoCurrent);
									telem.system.RSSI       = (int8_t)(system_pkt.RSSI);
									telem.system.boardT     = (int8_t)(system_pkt.AvionicsTemp);

									telem.navigation.APon   = status_pkt.GlobalOn;
									telem.navigation.MAmode = status_pkt.MAMode;
									telem.navigation.APmode = status_pkt.APMode;
									telem.navigation.waypt  = status_pkt.WaypointTo;

									telem.failure.aeroTermination     = status_pkt.AeroTermination;
									telem.failure.flightTermination   = status_pkt.FlightTermination;
									telem.failure.deadman             = status_pkt.Deadman;
									telem.failure.gpsTimeout          = status_pkt.GPSTimeout;
									telem.failure.commTimeout         = status_pkt.CommTimeout;
									telem.failure.manual_engineKill   = status_pkt.ManualEngineKill;
									telem.failure.auto_engineKill     = status_pkt.AutoEngineKill;
									telem.failure.operator_engineKill = status_pkt.OperatorEngineKill;

									telem.failure.operator_engineKill = status_pkt.OperatorEngineKill;

									telem.navigation.targetAltCmd = status_pkt.LoopStatus1;
									telem.navigation.targetAlt = status_pkt.LoopTargets[ LOOP_CMD_ALT ];
									telem.navigation.targetIASCmd = status_pkt.LoopStatus2;
									telem.navigation.targetIAS = status_pkt.LoopTargets[ LOOP_CMD_TAS ];

									break;

								case SYSTEM_STATUS_LO_RES:
									if(verbose)
										cout << getName() << "::parse() - SYSTEM_STATUS_LO_RES" << endl;
									break;

									/*DecodeTelemetryPacket(&autopilot_pkt, &pkt_data);
									telemetry.pos.lat=pkt_data.GPS.Latitude*180/M_PI;
									telemetry.pos.lon=pkt_data.GPS.Longitude*180/M_PI;
									telemetry.track.tru=pkt_data.GPS.Direction*180/M_PI;
									telemetry.airdata.trueAirSpeed=(int16_t)pkt_data.TAS;
									telemetry.airdata.barometricAlt=(int32_t)pkt_data.Alt;
									break;
								case AUTOPILOT_COMMAND:
									//if(verbose)
									cout << getName() << "::parse() - AUTOPILOT_COMMAND" << endl;
									telemetry.navigation.value = (uint8_t)DecodeAutopilotControlPacket(&autopilot_pkt, &autopilot_cmd);
									telemetry.system.availableForCommand = autopilot_cmd.GlobalOn;
									break;
								case CONTROL_DATA:
									//if(verbose)
									cout << getName() << "::parse() - CONTROL" << endl;
									DecodeControlDataPacket(&autopilot_pkt, &pkt_data, NULL, NULL);
									telemetry.angles.roll=(int16_t)pkt_data.Euler[ROLL_AXIS];
									telemetry.angles.pitch=(int16_t)pkt_data.Euler[PITCH_AXIS];
									telemetry.angles.yaw=(int16_t)pkt_data.Euler[YAW_AXIS];
									break;
								case SYSTEM_DIAGNOSTIC:
									//if(verbose)
									cout << getName() << "::parse() - SYSTEM DIAGNOSTIC" << endl;
									DecodeSystemDiagnosticPacket(&autopilot_pkt, &pkt_data);
									telemetry.system.twelveV=(int16_t)pkt_data.System.InputVoltage;
									break;*/

								case TRACK: {
									UInt8 Waypoint = autopilot_pkt.Data[0];

                  if((int)Waypoint == 0){lost_comm = TRUE; break;}

									if(verbose)
										cout << getName() << "::parse() - TRACK -- waypoint=" 
											  << (int)Waypoint << " lastTrack=" << (int)lastTrackWaypoint << endl;

									// if a user waypoint, return acknowledgement to last track command
									// FIXME with correct return to addr in ACK
									if( Waypoint >= USER_WAYPOINT_MIN && Waypoint <= USER_WAYPOINT_MAX ) {
										if( Waypoint == lastTrackWaypoint )
											ACK(INBOARD_TX);

									// if not a user waypoint
									} //else { // if( Waypoint < USER_WAYPOINT_MIN || Waypoint > USER_WAYPOINT_MAX ) {
										// if there is a difference in received track command and last waypoint
										if( lastTrackWaypoint != Waypoint ) {
											CMD_CMD(fifo_msg.data)  = CMD_RETURN;
											CMD_NUM(fifo_msg.data)  = 0;
											CMD_CSUM(fifo_msg.data) = 0;
											CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
											fifo_msg.size           = CMD_SIZE(fifo_msg.data);
											fifo_msg.dest.s_addr    = shmPtr->clients.getAddr(me).s_addr;
											writeFifo(INBOARD_TX);
											cout << " -- sent CMD_RETURN: " << endl;
										}
								//	}
									lastTrackWaypoint = Waypoint;

								} break;

								case WAYPOINT: {
									//cout <<  getName() << "::parse() - WAYPOINT";
									FPPoint_t pPoint;
									UInt8     index = DecodeWaypointPacket(&autopilot_pkt, &pPoint);

									if( index == 0xFF ) {
										if(verbose)
											cout << " --- invalid packet" << endl;
									} else {
										received_waypoint = index;
										//printf(" --- waypoint index=%i", index);
										//-- copy to locally maintained flight-plan --//
										piccolo_waypoints[index] = pPoint;
										SetFPMaskBit( active_wp_mask, index );

/*
										//-- check for onboard generated plan --//
										if( GetFPMaskBit(waypoint_mask, index) ) { 
											// if onboard generated, track the flight plan 
											if( index >= USER_WAYPOINT_MIN && index <= USER_WAYPOINT_MAX ) 
											{
												//printf(" was part of an onboard flight plan - tracking\n" );

												ClearAllFPMaskBits( waypoint_mask );
												autopilot_pkt.Size    = 2;
												autopilot_pkt.Data[0] = index; // select minimum waypoint # in plan
												autopilot_pkt.Data[1] = 0;     // join plan at closest segment

												MakeAutopilotPacket(&autopilot_pkt, TRACK, 2);
												MakePacket(&out_stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
												(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
												memcpy((void *)(fifo_msg.data), (void *)&out_stream_pkt, out_stream_pkt.Size + STREAM_PKT_OVERHEAD);
												fifo_msg.size = out_stream_pkt.Size + STREAM_PKT_OVERHEAD;
												writeFifo(OUTBOARD_TX);

												lastTrackWaypoint = index;
											} 
										}
*/
									}
									//printf("\n");
								} break;

								case WAYPOINT_LIST: {
									//cout << getName() << "::parse() - WAYPOINT_LIST" << endl;
									FPMask_t Mask;
									UInt8 Flags = DecodeWaypointListPacket(&autopilot_pkt, Mask);

									//-- if they are different sets, request new set of waypoints --//
									if( Flags != 0xFF && CompareFPMask(Mask, active_wp_mask) != 0 ) {
										CopyFPMask(Mask, active_wp_mask);
										
										// request active list of waypoints 
										FormWaypointListPacket(&autopilot_pkt, active_wp_mask, REQUEST_WAYPOINTS);
										MakePacket(&out_stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
												(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + AUTOPILOT_PKT_OVERHEAD);
										memcpy((void *)(fifo_msg.data), (void *)&out_stream_pkt, out_stream_pkt.Size + STREAM_PKT_OVERHEAD);
										fifo_msg.size = out_stream_pkt.Size + STREAM_PKT_OVERHEAD;
										writeFifo(OUTBOARD_TX);
									}
									break;
								} 
								default:
									//if(verbose)
										//cout << getName() << "::parse() - UNKNOWN AUTOPILOT PACKET TYPE [" 
										     //<< (int)autopilot_pkt.PktType << "]" << endl;
									break;
							}
						}
#ifdef DEBUG_PRINT
						printf("LookForAutopilotPacketInBuf: valid=%i numused=%i in_stream_pkt.Size=%i ind=%i\n", 
						        valid, (uint32_t)numused, (uint32_t)in_stream_pkt.Size, ind);
#endif
					} while(valid && ind < in_stream_pkt.Size); 
				} break;
				case MET_STREAM:
					if(verbose)
						cout << getName() << "::parse() - MET_STREAM: " << endl;
					break;
				default:
					if(verbose)
						cout << getName() << "::parse() - UNKNOWN STREAM [" << (int)in_stream_pkt.Stream << "]" << endl;
			}

			ret_val += autopilot_pkt.PktType;

#ifdef DEBUG_PRINT
			cout << "Processed stream packet in buffer: lookFor=" << lookFor << " n=" << n << endl;
#endif
		} 
	}

	//-- send down active flight plans --//
	if( received_waypoint >= 0 ) {
		FPMask_t Mask;
		uint8_t nPoints = GetPointsInPlan( piccolo_waypoints, Mask, received_waypoint); 
		if( nPoints <= 0 ) {
			cout << getName() << "::parse() - received_waypoint >= 0 but nPoints <= 0 " << endl;
			return ret_val;
		} 
	
		Waypt_t myWaypoints[nPoints];

		int ind = 0;
		int32_t i = FindFirstFPMaskBitSet(Mask);
		while(i >= 0 && i < NUM_FP_POINTS)
		{
			myWaypoints[ind].pos.lat  = piccolo_waypoints[i].Point.Lat * 180.0/M_PI;
			myWaypoints[ind].pos.lon  = piccolo_waypoints[i].Point.Lon * 180.0/M_PI;
			myWaypoints[ind].pos.alt  = piccolo_waypoints[i].Point.Alt;
			myWaypoints[ind].orbitRad = (int16_t)(piccolo_waypoints[i].OrbitRadius * (piccolo_waypoints[i].Direction ? -1 : 1));
			myWaypoints[ind].next     = piccolo_waypoints[i].Next;
			myWaypoints[ind].number   = i;

			i = FindNextFPMaskBitSet(Mask, (uint32_t)i);
			ind++;
			if( ind > nPoints ) {
				cout << getName() << "::parse() -  using more waypoints than in flight plan" << endl;
				return ret_val;
			}
		}

		// is any one subscribed to CMD_FP_SUBS
		if( shmPtr->clients.getSubscriptionsTo(CMD_FP_SUBS,0) == NULL)
			return ret_val;

		//if(verbose)
			//cout << getName() << "::parse() - sending flightplan with " << (int) nPoints << " points to clients" << endl;

		CMD_CMD(fifo_msg.data)  = CMD_FP_SUBS;
		CMD_NUM(fifo_msg.data)  = sizeof(Waypt_t) * nPoints;
		memcpy( CMD_DATAPTR(fifo_msg.data), myWaypoints, CMD_NUM(fifo_msg.data) );
		CMD_CSUM(fifo_msg.data) = 0;
		CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
		fifo_msg.size = CMD_SIZE(fifo_msg.data);

		Client * a_client;
		i = 0;
		while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_FP_SUBS,i)) != NULL)
		{
			//cout << " " << a_client->name << endl ;
			fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
			fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
			writeFifo(INBOARD_TX);
			i++;
		}
	} // send down waypoint

	return ret_val;
}

void PiccoloParser::sendTelem() {
	//Client * me = shmPtr->clients.getClient(0);

	CMD_CMD(fifo_msg.data)  = CMD_TELEM;
	CMD_NUM(fifo_msg.data)  = sizeof(Telemetry_t);
	CopyStructToBuf(telem, CMD_DATAPTR(fifo_msg.data) );
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
	fifo_msg.size = CMD_SIZE(fifo_msg.data);

	Client * a_client;
	int i=0;
	while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_TELEM,i)) != NULL)
	{
		Client * me = shmPtr->clients.getClient(0);
		fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
		fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
		writeFifo(INBOARD_TX);
		i++;
	}

	/*CMD_CMD((fifo_msg.data)) = CMD_GPS_TELEM;  ///FIXME!!
	CMD_NUM((fifo_msg.data)) = sizeof(GPSTelem_t);
	CopyStructToBuf(me->telemetry.gps, CMD_DATAPTR(fifo_msg.data));
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
	fifo_msg.size = CMD_SIZE(fifo_msg.data);

	i=0;
	while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_TELEM,i)) != NULL)
	{
		Client * me = shmPtr->clients.getClient(0);
		fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
		fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
		writeFifo(INBOARD_TX);
		i++;
	}*/

	CMD_CMD(fifo_msg.data)  = CMD_WIND;
	CMD_NUM(fifo_msg.data)  = sizeof(WindData_t);
	CopyStructToBuf(wind, CMD_DATAPTR(fifo_msg.data) );
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
	fifo_msg.size = CMD_SIZE(fifo_msg.data);

	i=0;
	while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_WIND,i)) != NULL)
	{
		Client * me = shmPtr->clients.getClient(0);
		fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
		fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
		writeFifo(INBOARD_TX);
		i++;
	}
}

void PiccoloParser::parse(const Command &cmd)
{
	//cout << "PiccoloParser::parse - command parse [" << (int)cmd.cmd() << ":" << cmd.getName() << "]" << endl;
	//cout << "  src=" << inet_ntoa(fifo_msg.src) << endl;
	//cout << "  dest=" << inet_ntoa(fifo_msg.dest) << endl;

	//cmd.copyToBuf((uint8_t*)fifo_msg.data);
	char * dataPtr = (char *)CMD_DATAPTR(fifo_msg.data);

	StreamPkt_t stream_pkt;
	AutopilotPkt_t autopilot_pkt;

	Client * a_client    = shmPtr->clients.getClient(fifo_msg.src);
	//Client * to_client   = shmPtr->clients.getClient(fifo_msg.dest);
	Client * me          = shmPtr->clients.getClient(0);

	switch(cmd.cmd() ) 
	{
		case CMD_FP: {
			int nPoints = (cmd.size()/sizeof(Waypt_t));
			int minID = 65535;

			if(verbose) 
				cout << getName() << "::cmd_parse - waypoint list received: nPoints=" << nPoints  <<endl;

			// our format of waypoints
			Waypt_t * waypoint = (Waypt_t *)dataPtr;
			Waypt_t myWaypoints[nPoints];

			// piccolo format of waypoints
			FPPoint_t tempPoint;
			FPlan_t   localPlan;

			ClearAllFPMaskBits(waypoint_mask);

			for(int i = 0; i < nPoints; i++) 
			{
				//-- Check that user is only modifying user wayponts
/*
				if( waypoint[i].number < USER_WAYPOINT_MIN ||
						waypoint[i].number > USER_WAYPOINT_MAX ||
						waypoint[i].next   < USER_WAYPOINT_MIN ||
						waypoint[i].next   > USER_WAYPOINT_MAX ) {
					NACK(INBOARD_TX);
					printf("%s::cmd_parse() - waypoint not a user waypoint to edit\n", name.c_str());

					return;
				}
*/

/*
				//-- Check Table Mountain boundary conditions --//
				if( waypoint[i].pos.lat > USER_MAX_LAT ||
						waypoint[i].pos.lat < USER_MIN_LAT ||
						waypoint[i].pos.lon > USER_MAX_LON ||
						waypoint[i].pos.lon < USER_MIN_LON ||
						waypoint[i].number  < USER_WAYPOINT_MIN ||
						waypoint[i].next  < USER_WAYPOINT_MIN ||
						waypoint[i].number  > USER_WAYPOINT_MAX ||
						waypoint[i].next  > USER_WAYPOINT_MAX ||
						waypoint[i].pos.alt < USER_MIN_ALT      ||
						waypoint[i].pos.alt > USER_MAX_ALT ) {

					NACK(INBOARD_TX);
					printf("%s::cmd_parse() - waypoint out of bounds (%i, %i, %f, %f, %f)\n",
						name.c_str(), waypoint[i].number, waypoint[i].next,
						waypoint[i].pos.lat, waypoint[i].pos.lon, waypoint[i].pos.alt);

					return;
				}
*/

				// copy waypoints into local buffer to avoid memory errors with fifo_msg being used
				myWaypoints[i] = waypoint[i];
				float orbitRad = abs( myWaypoints[i].orbitRad );
				bool orbitDir  = ( myWaypoints[i].orbitRad < 0 ? 1 : 0 );
				int wpInd      = myWaypoints[i].number; 

				// FIXME - should check to see if waypoint is already defined in a plan. if so
				// we would want to delete the plan before uploading these current waypoints. Otherwise
				// the piccolo will be left with orphaned waypoints 
				if( !IsFPPointValid( &piccolo_waypoints[wpInd] ) ) {
					FPMask_t checkMask;
					UInt8 numPoints_toRemove = GetPointsInPlan(piccolo_waypoints, checkMask, wpInd);
					if( numPoints_toRemove > 0 ) {
						printf("%s::cmd_parse() - will have orphand waypoints !!! should fix this \n", name.c_str());
						// FIXME -- do something here
					}
				}

				//-- copy into piccolo's waypoint format --//
				tempPoint.Point.Lat     = myWaypoints[i].pos.lat*M_PI/180.0;
				tempPoint.Point.Lon     = myWaypoints[i].pos.lon*M_PI/180.0;
				tempPoint.Point.Alt     = myWaypoints[i].pos.alt;
				tempPoint.WindInterval  = 0.0;
				tempPoint.OrbitRadius   = orbitRad;
				tempPoint.OrbitTime     = 0;
				tempPoint.OrbitBelowAlt = 0;
				tempPoint.OrbitAboveAlt = 0;
				tempPoint.Hover         = 0;
				tempPoint.Parachute     = 0;
				tempPoint.Drop          = 0;
				tempPoint.Direction     = orbitDir;
				tempPoint.Camera        = 0;
				tempPoint.Land          = 0;
				tempPoint.Slope         = 0;
				tempPoint.Lights        = 0;
				tempPoint.PreTurn       = 1;
				tempPoint.User          = 0;
				tempPoint.Next          = myWaypoints[i].next;

				//-- check point be valid --//
				if( !IsFPPointValid( &tempPoint ) ) {
					NACK(INBOARD_TX);
					printf("%s::cmd_parse() - waypoint is not valid (%f, %f, %f, id=%i, next=%i)\n",
							name.c_str(), myWaypoints[i].pos.lat, myWaypoints[i].pos.lon, 
							myWaypoints[i].pos.alt, myWaypoints[i].number, myWaypoints[i].next);

					ClearAllFPMaskBits(waypoint_mask);
					return;
				}

				//-- set fp bit mask --//
				minID = ( wpInd < minID ? wpInd : minID);
				SetFPMaskBit( waypoint_mask, wpInd );

				//-- copy to local storage area --//
				localPlan[wpInd] = tempPoint;
			}
			
			//-- check for closed flight plan --//
			if( !CheckFlightPlanClosure( localPlan, minID ) ) {
				NACK(INBOARD_TX);
				printf("%s::cmd_parse() - waypoint plan does not close\n", name.c_str());
				// FIXME -- need to clear the piccolo_waypoints that were overwritting
				return;
			}

			//-- format block transfer --//
			if( nPoints > 1) {
				FormWaypointListPacket(&autopilot_pkt, waypoint_mask, WAYPOINT_BLOCK);
				MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + AUTOPILOT_PKT_OVERHEAD);
				memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
				fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
				writeFifo(OUTBOARD_TX);
			}
			
			//-- send waypoints --//
			for(int i = 0; i < nPoints; i++) {
				int wpInd = myWaypoints[i].number;
				FormWaypointPacket(&autopilot_pkt, &(localPlan[wpInd]), wpInd);
				MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						(UInt8 *)&autopilot_pkt, autopilot_pkt.Size + AUTOPILOT_PKT_OVERHEAD);
				memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
				fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
				writeFifo(OUTBOARD_TX);
				if(verbose)
					cout << getName() << "::cmd_parse() - sent waypoint: ind=" <<  (int)i 
						  << " num=" << wpInd << endl;
			}

			// FIXME need to check waypoint echo is valid before sending ack/nack
			ACK(INBOARD_TX);

		} break;

		case CMD_GOTO_WP: {
			GotoWP_t *wp = (GotoWP_t *)CMD_DATAPTR(fifo_msg.data);
			lastTrackWaypoint = wp->ind;

			//if( verbose )
        cout << getName() << "::cmd_parse() - received CMD_GOTO_WP: WP=" << lastTrackWaypoint << endl;

			// reply back to sender
			ACK(INBOARD_TX);

			//-- Track the lst waypoint target --//
			autopilot_pkt.Size    = 2;
			autopilot_pkt.Data[0] = lastTrackWaypoint;
			autopilot_pkt.Data[1] = 1;

			MakeAutopilotPacket(&autopilot_pkt, TRACK, 2);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

			if( a_client != me ) {
				CMD_CMD(fifo_msg.data)  = CMD_RETURN;
				CMD_NUM(fifo_msg.data)  = 0;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
				fifo_msg.size           = CMD_SIZE(fifo_msg.data);
				fifo_msg.dest.s_addr    = shmPtr->clients.getAddr(me).s_addr;
				writeFifo(INBOARD_TX);
				cout << " -- sent CMD_RETURN" << endl;
			}

		} break;

		case CMD_IAS_LOOP: {
			GotoIAS_t ias;
			memcpy(&ias,(GotoIAS_t *)CMD_DATAPTR(fifo_msg.data),sizeof(GotoIAS_t));

			//if( verbose )
        cout << getName() << "::cmd_parse() - received CMD_IAS_LOOP: IAS=" << ias.value << " CONTROL=" << (int)(ias.control) << endl;

			// reply back to sender
			ACK(INBOARD_TX);

			AutopilotLoopCmd_t pCmd;
			pCmd.Loop = LOOP_CMD_TAS;
			pCmd.Control = ias.control;
			pCmd.Value = ias.value;

			cout << "pCmd.Control=" << (int)(pCmd.Control) << endl;

			FormAutopilotLoopControlPacket(&autopilot_pkt, &pCmd);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

		} break;

		case CMD_ALT_LOOP: {
			GotoAlt_t alt;
			memcpy(&alt,(GotoAlt_t *)CMD_DATAPTR(fifo_msg.data),sizeof(GotoAlt_t));

			//if( verbose )
        cout << getName() << "::cmd_parse() - received CMD_ALT_LOOP: ALT=" << alt.value << " CONTROL=" << (int)(alt.control) << endl;

			// reply back to sender
			ACK(INBOARD_TX);

			AutopilotLoopCmd_t pCmd;
			pCmd.Loop = LOOP_CMD_ALT;
			pCmd.Control = alt.control;
			pCmd.Value = alt.value;

			FormAutopilotLoopControlPacket(&autopilot_pkt, &pCmd);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

		} break;

		case CMD_RETURN: {
			//if( verbose )
				cout << getName() << "::cmd_parse() -- received CMD_RETURN: sending UA to waypoint=" 
				     << lastTrackWaypoint << endl;

			//-- Track the lst waypoint target --//
			if(lastTrackWaypoint >= 0) {
				autopilot_pkt.Size    = 2;
				autopilot_pkt.Data[0] = lastTrackWaypoint;
				autopilot_pkt.Data[1] = 1;

				MakeAutopilotPacket(&autopilot_pkt, TRACK, 2);
				MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
									 (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
				memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
				fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
				writeFifo(OUTBOARD_TX);
			}
		} break;

    case CMD_BANK_LOOP: {
      if(lost_comm) break;
			GotoBank_t * bank_cmd = (GotoBank_t*)CMD_DATAPTR(fifo_msg.data);

			AutopilotLoopCmd_t pCmd;
			pCmd.Loop = LOOP_CMD_BANK;
			pCmd.Control = bank_cmd->control;
			pCmd.Value = bank_cmd->value;

			FormAutopilotLoopControlPacket(&autopilot_pkt, &pCmd);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

			/*GotoBank_t * bank_t = (GotoBank_t*)CMD_DATAPTR(fifo_msg.data);

			float bank = bank_t->value;

			uint8_t * fptr = (uint8_t*)&(bank);

			//-- command heading rate --//
			autopilot_pkt.Size = 8;
			autopilot_pkt.Data[0]=2;  //turn rate loop
			autopilot_pkt.Data[1]=1;
			autopilot_pkt.Data[4]=fptr[3];
			autopilot_pkt.Data[5]=fptr[2];
			autopilot_pkt.Data[6]=fptr[1];
			autopilot_pkt.Data[7]=fptr[0];

      cout << "New bank command: " << bank << endl;
			MakeAutopilotPacket(&autopilot_pkt, AUTOPILOT_LOOP, 8);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);*/
      } break;

		case CMD_AP_RATES: {
			//cout << getName() << "::cmd_parse - rates received" <<endl;
			APRates_t * ap_rates = (APRates_t*)CMD_DATAPTR(fifo_msg.data);

			float turn_rate = ap_rates->turn_rate;
			float velocity =  ap_rates->velocity;

      float heading = turn_rate*0;  //FIXME -- needs to accomodate commanded turn rate

			uint8_t * fptr = (uint8_t*)&(heading);

			//-- command heading rate --//
			autopilot_pkt.Size = 8;
			autopilot_pkt.Data[0]=4;  //turn rate loop
			autopilot_pkt.Data[1]=1;
			autopilot_pkt.Data[4]=fptr[3];
			autopilot_pkt.Data[5]=fptr[2];
			autopilot_pkt.Data[6]=fptr[1];
			autopilot_pkt.Data[7]=fptr[0];

			MakeAutopilotPacket(&autopilot_pkt, AUTOPILOT_LOOP, 8);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

			fptr = (uint8_t*)&(velocity);

			autopilot_pkt.Size = 8;
			autopilot_pkt.Data[0]=0;  //velocity loop
			autopilot_pkt.Data[1]=1;
			autopilot_pkt.Data[4]=fptr[3];
			autopilot_pkt.Data[5]=fptr[2];
			autopilot_pkt.Data[6]=fptr[1];
			autopilot_pkt.Data[7]=fptr[0];

			MakeAutopilotPacket(&autopilot_pkt, AUTOPILOT_LOOP, 8);
			MakePacket(&stream_pkt, network_id, OI_ADDRESS, AUTOPILOT_STREAM, 0, 0, 0, 
						     (UInt8 *)&autopilot_pkt, autopilot_pkt.Size + STREAM_PKT_OVERHEAD);
			memcpy((void *)(fifo_msg.data), (void *)&stream_pkt, stream_pkt.Size + STREAM_PKT_OVERHEAD);
			fifo_msg.size = stream_pkt.Size + STREAM_PKT_OVERHEAD;
			writeFifo(OUTBOARD_TX);

		} break;

		case CMD_STREAM_REQ: {
			StreamReq_t * request = (StreamReq_t *)CMD_DATAPTR(fifo_msg.data);

			printf("Stream Request [%x] from ", request->id);
			cout << inet_ntoa(request->client) << " on port "
				<< request->port << " with action "
				<< request->send_data << endl;

			Client * a_client = shmPtr->clients.getClient(fifo_msg.src);
			if( a_client == NULL ) {
				cout << getName() << "::cmd_parse - fifo_msg.src not found in client list" << endl;
				break;
			}

			//-- if request subscription to flight plans, download active set --//
			if(request->send_data && request->id == CMD_FP_SUBS) {
				//if(verbose)
					cout << getName() << "::cmd_parse - sending " << a_client->name 
					     << " current flight plans "  << endl;
	
				int nPoints = 0;
				int pInd[NUM_FP_POINTS];

				// first, count number of valid points
				for(int i=0; i < NUM_FP_POINTS; i++) {
					pInd[i] = -1;
					if( IsFPPointValid( &(piccolo_waypoints[i]) ) ) {
						pInd[nPoints] = i;
						nPoints++;
					}
				}

				Waypt_t myWaypoints[nPoints];

				int ind;
				for(int i=0; i < nPoints; i++) 
				{
					ind = pInd[i];
					myWaypoints[i].pos.lat  = piccolo_waypoints[ind].Point.Lat * 180.0/M_PI;
					myWaypoints[i].pos.lon  = piccolo_waypoints[ind].Point.Lon * 180.0/M_PI;
					myWaypoints[i].pos.alt  = piccolo_waypoints[ind].Point.Alt;
					myWaypoints[i].orbitRad = (int16_t)(piccolo_waypoints[ind].OrbitRadius * (piccolo_waypoints[ind].Direction ? -1 : 1));
					myWaypoints[i].next     = piccolo_waypoints[ind].Next;
					myWaypoints[i].number   = ind;
				}

				Client * me = shmPtr->clients.getClient(0);

				CMD_CMD(fifo_msg.data)  = CMD_FP_SUBS;
				CMD_NUM(fifo_msg.data)  = sizeof(Waypt_t) * nPoints;
				memcpy( CMD_DATAPTR(fifo_msg.data), myWaypoints, CMD_NUM(fifo_msg.data) );
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size           = CMD_SIZE(fifo_msg.data);
				fifo_msg.dest.s_addr    = fifo_msg.src.s_addr;
				fifo_msg.src.s_addr     = shmPtr->clients.getAddr(me).s_addr;
				writeFifo(INBOARD_TX);
			}

		} break;

		case CMD_CONNECT: {  //FIXME -- should probably first check to make sure that the serialThread doesn't push this to the piccolo
				writeFifo(OUTBOARD_TX);
		} break;

		case CMD_ACK: {  //FIXME -- should probably first check to make sure that the serialThread doesn't push this to the piccolo
				writeFifo(INBOARD_TX);
		} break;

		case CMD_FAIL: {  //FIXME -- should probably first check to make sure that the serialThread doesn't push this to the piccolo
				writeFifo(INBOARD_TX);
		} break;

		default:
			//cout << getName() << "::cmd_parse() - " << cmd.getName() << " is not handled" << endl;
			break;
	}
}
